#define TWN_ALONE_PATH_F
#include "Twn_Path_Fill.h"

/****************************************************************************************/
/*																						*/
/*		Function:		PathStepAdd														*/
/*		Description:	Add a point in the path array in the F.PATH DAFS file (405).	*/
/*		Input:			point_X		-	X physical coordinate of the new point.			*/
/*						point_Y		-	Y physical coordinate of the new point.			*/
/*						type		-	0 - Planned trajectory							*/
/*										1 - Executed trajectory							*/
/*		Return:			R_NORMAL	-	Point inserted with success.					*/
/*						R_FAILED	-	Write failure or out of array range.			*/
/*																						*/
/****************************************************************************************/
RETCOND PathStepAdd(int mastType, float point_X, float point_Y, int type){
	RETCOND		retCnd;
	path_point	path[MAX_STEPS];
	int			i,len;
	int			recNum;
	FILEIDX		filePath;

	filePath.num = F_PATH;
	
	if(mastType ==MAIN_MAST) {
		if(type == PATH_TYPE_PLAN)
			recNum = 1;
		else
			recNum = 3;
	}
	else if((mastType == FALC_MAST) ||(mastType == DMP_MAST)){
		if(type == PATH_TYPE_PLAN)
			recNum = 2;
		else
			recNum = 4;

	}

	retCnd = FileRead(&filePath, recNum, 1, &path, sizeof(path));
	if(retCnd != R_NORMAL){
		return R_FAILED;
	}

	retCnd = PathLen(mastType, type, &len);
	if(retCnd != R_NORMAL){
		return R_FAILED;
	}
	
	for(i = len; i < MAX_STEPS; i++){
		path[i].PhysX = point_X;
		path[i].PhysY = point_Y;
	}

	retCnd = FileWrite(&filePath, recNum, 1, path, sizeof(path));
	if(retCnd != R_NORMAL){
		return R_FAILED;
	}

	return(R_NORMAL);
}

/****************************************************************************************/
/*																						*/
/*		Function:		PathInit														*/
/*		Description:	Initialize the path array with zeroes						.	*/
/*		Return:			R_NORMAL	-	Point inserted with success.					*/
/*						R_FAILED	-	Write failure.									*/
/*																						*/
/****************************************************************************************/
RETCOND PathInit(GENIDX mastType, int type){
	RETCOND		retCnd;
	path_point	path[MAX_STEPS];
	int			i;
	int			recNum;
	FILEIDX		filePath;

	if(mastType ==MAIN_MAST) {
		if(type == PATH_TYPE_PLAN)
			recNum = 1;
		else
			recNum = 3;
	}
	else if((mastType == FALC_MAST) ||(mastType == DMP_MAST)){
		if(type == PATH_TYPE_PLAN)
			recNum = 2;
		else
			recNum = 4;

	}

	filePath.num = F_PATH;
	for(i = 0; i < MAX_STEPS; i++){
		path[i].PhysX = 0.0F;
		path[i].PhysY = 0.0F;
	}

	retCnd = FileWrite(&filePath, recNum, 1, path, sizeof(path));
	if(retCnd != R_NORMAL){
		return R_FAILED;
	}

	return(R_NORMAL);
}

/****************************************************************************************/
/*																						*/
/*		Function:		PathLen															*/
/*		Description:	Retrieve the actual path lenght.							.	*/
/*		Output:			lenght		-	Actual lenght (number of points).				*/
/*		Return:			R_NORMAL	-	Point inserted with success.					*/
/*						R_FAILED	-	Write failure.									*/
/*																						*/
/****************************************************************************************/
RETCOND PathLen(GENIDX mastType, int type, int *lenght){
	RETCOND		retCnd;
	path_point	path[MAX_STEPS];
	int			i;
	int			recNum;
	FILEIDX		filePath;

	filePath.num = F_PATH;

	*lenght = 0;

	if(mastType ==MAIN_MAST) {
		if(type == PATH_TYPE_PLAN)
			recNum = 1;
		else
			recNum = 3;
	}
	else if((mastType == FALC_MAST) ||(mastType == DMP_MAST)){
		if(type == PATH_TYPE_PLAN)
			recNum = 2;
		else
			recNum = 4;

	}

	retCnd = FileRead(&filePath, recNum, 1, &path, sizeof(path));
	if(retCnd != R_NORMAL){
		return R_FAILED;
	}

	for(i = 0; i < MAX_STEPS; i++){
		if((path[i].PhysX == 0) && (path[i].PhysY == 0) && (i == 0)){
			*lenght = 0;
			break;
		}
		else if((path[i].PhysX == path[i + 1].PhysX) && (path[i].PhysY == path[i + 1].PhysY) && (path[i].PhysX != 0) && (path[i].PhysY != 0)){
			*lenght = i + 1;
			break;
		}
	}

	return(R_NORMAL);
}
